#include <ros/ros.h>
#include "UWB_message_type.h"
#include <nlink_parser/LinktrackNodeframe0.h>
#include <nlink_parser/LinktrackNodeframe2.h>
#include <nlink_parser/LinktrackNodeframe3.h>
#include <nlink_parser/LinktrackNode0.h>
#include <std_msgs/String.h>
#include <eigen3/Eigen/Dense>
#include <geometry_msgs/PoseStamped.h>



ros::Publisher pose_pub;
// ros::Publisher pose_raw_pub;

double camera_frame_rate;

uint32_t g_systemTime = 0;
double ros_time_minus_system_time = -UINT32_MAX/1000.0;
geometry_msgs::PoseStamped latest_pose;
Eigen::Vector3d world_to_cam_position_sum(0,0,0);
Eigen::Matrix4Xd world_to_cam_orientation_sum(4,0);

ros::Time convert_system_time(uint32_t sys_time)
{
    return ros::Time(sys_time/1000.0 + ros_time_minus_system_time);
}

geometry_msgs::Point weighted_average(geometry_msgs::Point p1, double w1,geometry_msgs::Point p2, double w2)
{
    geometry_msgs::Point p;
    p.x = p1.x*w1 + p2.x*w2;
    p.y = p1.y*w1 + p2.y*w2;
    p.z = p1.z*w1 + p2.z*w2;
    return p;
}

geometry_msgs::Quaternion weighted_average(geometry_msgs::Quaternion q1, double w1,geometry_msgs::Quaternion q2, double w2)
{
    geometry_msgs::Quaternion q;
    Eigen::Vector4d q_averaged;
    Eigen::Matrix4Xd Q(4,2);
    Q.col(0) = Eigen::Vector4d(q1.x, q1.y, q1.z, q1.w);
    Q.col(1) = Eigen::Vector4d(q2.x, q2.y, q2.z, q2.w);
    Eigen::EigenSolver<Eigen::Matrix4d> es(Q * Q.transpose());
    Eigen::MatrixXcd evecs = es.eigenvectors();//获取矩阵特征向量4*4，这里定义的MatrixXcd必须有c，表示获得的是complex复数矩阵
    Eigen::MatrixXcd evals = es.eigenvalues();//获取矩阵特征值 4*1
    Eigen::MatrixXd evalsReal;//注意这里定义的MatrixXd里没有c
    evalsReal=evals.real();//获取特征值实数部分
    Eigen::MatrixXf::Index evalsMax;
    evalsReal.rowwise().sum().maxCoeff(&evalsMax);//得到最大特征值的位置
    q_averaged << evecs.real()(0, evalsMax), 
                    evecs.real()(1, evalsMax), 
                    evecs.real()(2, evalsMax), 
                    evecs.real()(3, evalsMax);//得到对应特征向量
    if(q_averaged.w() < 0)
        q_averaged = -q_averaged;
    q.x = q_averaged.x();
    q.y = q_averaged.y();
    q.z = q_averaged.z();
    q.w = q_averaged.w();
    return q;
}

void update_latest_pose(geometry_msgs::PoseStamped pose_new)
{
    double latest_time = latest_pose.header.stamp.toSec();
    double new_time = pose_new.header.stamp.toSec();
    if(latest_pose.header.stamp.sec == 0 && latest_pose.header.stamp.nsec == 0) 
    {
        latest_pose = pose_new;
        ROS_INFO("latest_pose: init success!");
        ROS_INFO_STREAM(latest_pose);
    }
    else if(latest_time - new_time > 1/camera_frame_rate)
    {
        ROS_ERROR_STREAM_THROTTLE(0.01, "pose_new REJECTED! latest_time: " << latest_time << " new_time: " << new_time);
    }
    else if(new_time - latest_time > 1/camera_frame_rate)
    {
        ROS_WARN_STREAM_THROTTLE(0.01, "pose_new ACCPTED! latest_time - new_time: " << latest_time - new_time);
        latest_pose = pose_new;
    }
    else 
    {
        double latest_time = latest_pose.header.stamp.toSec();
        double new_time = pose_new.header.stamp.toSec();
        latest_pose.header = (latest_time > new_time)? latest_pose.header : pose_new.header;
        double big_w,small_w;
        big_w = 0.5 + 0.5*std::abs(latest_time - new_time)/(1/camera_frame_rate);
        small_w = 0.5 - 0.5*std::abs(latest_time - new_time)/(1/camera_frame_rate);
        double weight_latest, weight_new;
        weight_latest = (latest_time > new_time)? big_w : small_w;
        weight_new = (latest_time > new_time)? small_w : big_w;
        latest_pose.pose.position = weighted_average(latest_pose.pose.position, weight_latest, pose_new.pose.position, weight_new);
        latest_pose.pose.orientation = weighted_average(latest_pose.pose.orientation, weight_latest, pose_new.pose.orientation, weight_new);
    }
    
}

void UWB_frame3_callback(const nlink_parser::LinktrackNodeframe3::ConstPtr &msg)
{
    g_systemTime = msg->system_time;
    ros_time_minus_system_time = std::max(ros_time_minus_system_time, ros::Time::now().toSec() - g_systemTime/1000.0);
}

void UWB_frame2_callback(const nlink_parser::LinktrackNodeframe2::ConstPtr &msg)
{
    g_systemTime = msg->system_time;
    ros_time_minus_system_time = std::max(ros_time_minus_system_time, ros::Time::now().toSec() - g_systemTime/1000.0);
}


void UWB_frame0_callback(const nlink_parser::LinktrackNodeframe0::ConstPtr &msg)
{
    if(ros_time_minus_system_time!=INT32_MIN/1000.0)
    {
        for(int i = 0; i < msg->nodes.size();i++)
        {
            if(msg->nodes[i].data.size() == sizeof(UWB_messages::ObservationMessage))
            {
                UWB_messages::ObservationMessage obs_msg(UWB_messages::convertToMessage<UWB_messages::ObservationMessage>(msg->nodes[i].data));
                geometry_msgs::PoseStamped pose_new;
                pose_new.header.stamp = convert_system_time(obs_msg.systemTime);
                pose_new.pose = obs_msg.get_geometry_pose_world();
                update_latest_pose(pose_new);
            }
        }
    }
}

void pose_pub_timer_callback(const ros::TimerEvent&)
{
    pose_pub.publish(latest_pose);
}



int main(int argc, char *argv[])
{
    ros::init(argc, argv, "pnp_MV_cam");
    ros::NodeHandle nh;
    pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 1);
    // pose_raw_pub = nh.advertise<geometry_msgs::PoseStamped>("/cam_raw/pose", 1);
    ros::Subscriber UWB_frame0_sub = nh.subscribe<nlink_parser::LinktrackNodeframe0>("/nlink_linktrack_nodeframe0",1,UWB_frame0_callback);
    ros::Subscriber UWB_frame2_sub = nh.subscribe<nlink_parser::LinktrackNodeframe2>("/nlink_linktrack_nodeframe2",1,UWB_frame2_callback);
    ros::Subscriber UWB_frame3_sub = nh.subscribe<nlink_parser::LinktrackNodeframe3>("/nlink_linktrack_nodeframe3",1,UWB_frame3_callback);
    if(!ros::param::get("camera_frame_rate", camera_frame_rate))
    {
        ROS_ERROR("ros::param::get(\"camera_frame_rate\", camera_frame_rate) FAILED!!! ");
        return 0;
    }
    double pose_pub_rate;
    if(!ros::param::get("pose_pub_rate", pose_pub_rate))
    {
        ROS_ERROR("ros::param::get(\"pose_pub_rate\", pose_pub_rate) FAILED!!! ");
        return 0;
    }

    ros::spinOnce();
    ros::Timer pose_pub_timer = nh.createTimer(ros::Duration(1/pose_pub_rate), pose_pub_timer_callback);
    ros::spin();
    return 0;
}